#ifndef _AMERICO_MATRIX_MATH_H_
#define _AMERICO_MATRIX_MATH_H_

#include "Matrix.h"

template <typename T>
Matrix22<T> abs(const Matrix22<T> &aMatrix)
{
	Matrix22<T> result(aMatrix);
	for (unsigned int index=0; index<2*2; ++index)
	{
		result[index] = abs(result[index]);
	}
	return result;
}

template <typename T>
Matrix33<T> abs(const Matrix33<T> &aMatrix)
{
	Matrix33<T> result(aMatrix);
	for (unsigned int index=0; index<3*3; ++index)
	{
		result[index] = abs(result[index]);
	}
	return result;
}

template <typename T>
Matrix44<T> abs(const Matrix44<T> &aMatrix)
{
	Matrix44<T> result(aMatrix);
	for (unsigned int index=0; index<4*4; ++index)
	{
		result[index] = abs(result[index]);
	}
	return result;
}

template <typename T>
Matrix44<T> operator*(const Matrix44<T> &aMatrix44, const Matrix33<T> &aMatrix33)
{
	Matrix44<T> result(aMatrix44);
	result *= aMatrix33;
	return result;
}

template <typename T>
Matrix44<T> &operator*=(Matrix44<T> &aMatrix44, const Matrix33<T> &aMatrix33)
{
	Matrix44<T> matrix(aMatrix44);
	for(unsigned int i=0; i<3; ++i)
	{
		for(unsigned int j=0; j<3; ++j)
		{
			aMatrix44[i + 4*j] = 0;
			for(unsigned int r=0; r<3; ++r)
			{
				aMatrix44[i + 4*j] += matrix[i + 4*r] * aMatrix33[r + 3*j];
			}
		}
	}
	return aMatrix44;
}

template <typename T>
Matrix44<T> operator*(Matrix33<T> &aMatrix33, const Matrix44<T> &aMatrix44)
{
	Matrix44<T> returnMatrix;

	returnMatrix[0] = aMatrix33[0] * aMatrix44[0] + aMatrix33[1] * aMatrix44[4] + aMatrix33[2] * aMatrix44[8];
	returnMatrix[1] = aMatrix33[0] * aMatrix44[1] + aMatrix33[1] * aMatrix44[5] + aMatrix33[2] * aMatrix44[9];
	returnMatrix[2] = aMatrix33[0] * aMatrix44[2] + aMatrix33[1] * aMatrix44[6] + aMatrix33[2] * aMatrix44[10];
	returnMatrix[3] = aMatrix44[3];

	returnMatrix[4] = aMatrix33[3] * aMatrix44[0] + aMatrix33[4] * aMatrix44[4] + aMatrix33[5] * aMatrix44[8];
	returnMatrix[5] = aMatrix33[3] * aMatrix44[1] + aMatrix33[4] * aMatrix44[5] + aMatrix33[5] * aMatrix44[9];
	returnMatrix[6] = aMatrix33[3] * aMatrix44[2] + aMatrix33[4] * aMatrix44[6] + aMatrix33[5] * aMatrix44[10];
	returnMatrix[7] = aMatrix44[7];

	returnMatrix[8] = aMatrix33[6] * aMatrix44[0] + aMatrix33[7] * aMatrix44[4] + aMatrix33[8] * aMatrix44[8];
	returnMatrix[9] = aMatrix33[6] * aMatrix44[1] + aMatrix33[7] * aMatrix44[5] + aMatrix33[8] * aMatrix44[9];
	returnMatrix[10] = aMatrix33[6] * aMatrix44[2] + aMatrix33[7] * aMatrix44[6] + aMatrix33[8] * aMatrix44[10];
	returnMatrix[11] = aMatrix44[11];

	returnMatrix[12] = aMatrix44[12];
	returnMatrix[13] = aMatrix44[13];
	returnMatrix[14] = aMatrix44[14];
	returnMatrix[15] = aMatrix44[15];

	return returnMatrix;
}

template <typename T>
Matrix33<T> Convert(const Matrix44<T> &aMatrix)
{
	Matrix33<T> temp;
	for (unsigned short i=0; i<3; i++)
	{
		for (unsigned short j=0; j<3; j++)
		{
			temp[i+3*j] = aMatrix[i+4*j];
		}
	}
	return temp;
}

template <typename T>
Matrix44<T> Convert(const Matrix33<T> &aMatrix)
{
	Matrix44<T> temp;
	for (unsigned short i=0; i<3; i++)
	{
		for (unsigned short j=0; j<3; j++)
		{
			temp[i+4*j] = aMatrix[i+3*j];
		}
	}
	return temp;
}

//template <typename T>
//Matrix44<T> LookAt(Vector3<T> aEye, Vector3<T> aAt, Vector3<T> aUp)
//{
//	D3DXMATRIX temp;
//	D3DXMatrixLookAtRH(&temp, &D3DXVECTOR3(aEye.x, aEye.y, aEye.z), &D3DXVECTOR3(aAt.x, aAt.y, aAt.z), &D3DXVECTOR3(aUp.x, aUp.y, aUp.z));
//	return Matrix44<T>::Convert(&temp[0]);
//}

template <typename T>
Matrix44<T> CreateLookAt(const Vector3<T> &aPosition, const Vector3<T> &aTarget, const Vector3<T> &anUp)
{
	Matrix44<T> retMatrix;
	CreateLookAt(retMatrix, aPosition, aTarget, anUp);
	return retMatrix;
}

template <typename T>
void CreateLookAt(Matrix44<T> &aMatrix, const Vector3<T> &aPosition, const Vector3<T> &aTarget, const Vector3<T> &anUp)
{
	Vector3<T> zaxis = (aTarget-aPosition).Normalize();
	Vector3<T> xaxis = anUp.Cross(zaxis).Normalize();
	Vector3<T> yaxis = zaxis.Cross(xaxis).Normalize();

	aMatrix[0] = xaxis.x; aMatrix[1] = xaxis.y; aMatrix[2] = xaxis.z = aMatrix[3] = 0;
	aMatrix[4] = yaxis.x; aMatrix[5] = yaxis.y; aMatrix[6] = yaxis.z = aMatrix[7] = 0;
	aMatrix[8] = zaxis.x; aMatrix[9] = zaxis.y; aMatrix[10] = zaxis.z = aMatrix[11] = 0;
	aMatrix[12] = -xaxis.Dot(aPosition); aMatrix[13] = -yaxis.Dot(aPosition); aMatrix[14] = -zaxis.Dot(aPosition);
	aMatrix[14] = 1;
}

template <typename T>
void CreateLookAtSafe(Matrix33<T> &aMatrix, const Vector3<T>& aNormalizedDirection)
{
	Vector3<T> perpendicularXZPlane(aNormalizedDirection.z, 0, -aNormalizedDirection.x);
	if(perpendicularXZPlane.z == 0 && perpendicularXZPlane.x == 0)
	{
		perpendicularXZPlane = Vector3f(-aNormalizedDirection.y, 0, 0);
	}
	perpendicularXZPlane.Normalize();
	Vector3<T> upVector = perpendicularXZPlane.Cross(aNormalizedDirection);

	upVector.Normalize();

	aMatrix[0] = perpendicularXZPlane.x;
	aMatrix[1] = perpendicularXZPlane.y;
	aMatrix[2] = perpendicularXZPlane.z;

	aMatrix[3] = -upVector.x;
	aMatrix[4] = -upVector.y;
	aMatrix[5] = -upVector.z;

	aMatrix[6] = aNormalizedDirection.x;
	aMatrix[7] = aNormalizedDirection.y;
	aMatrix[8] = aNormalizedDirection.z;
}

template <typename T>
void SetRotation(Matrix44<T> &aMatrix, const Matrix33<T> &aRotationMatrix)
{
	for (unsigned short i=0; i<3; i++)
	{
		for (unsigned short j=0; j<3; j++)
		{
			aMatrix[i+4*j] = aRotationMatrix[i+3*j];
		}
	}
}

template <typename T>
Vector3<T> GetZ( Matrix44<T> &aMatrix )
{
	return Vector3<T>( aMatrix[8], aMatrix[9], aMatrix[10] );
}

#endif